////
////  motionModel.cpp
////  markerARLib
////
////  Created by fg on 6/20/16.
////  Copyright © 2016 CvidKal. All rights reserved.
////
//
//#include "Motion_Model.hpp"
//#include <fstream>
//
//using namespace std;
//using namespace markerAR;
//int main()
//{
//    ifstream fin("/Users/fg/cycle/trajectoryCeres.txt");
//    Motion_Model mm(10);
//    double timestamp;
//    std::vector<RigidMotion> motions;
//    ofstream fout("/Users/fg/cycle/motion.txt");
//    while (fin>>timestamp) {
//        Pose p;
//        double t[3],r[3];
//        fin>> t[0]>>t[1]>>t[2]>>r[0]>>r[1]>>r[2];
//        p.t<<t[0],t[1],t[2];
//        ceres::EulerAnglesToRotationMatrix(r, 3, p.R.data());
//        p.R.transposeInPlace();
//        auto motion = mm.addPose(p, timestamp);
//        motions.push_back(motion);
//    };
//    for (int i=0; i<motions.size()-1; ++i) {
//        auto front = motions[i];
//        auto back = motions[i+1];
//        double t = back.timestamp - front.timestamp;
//        RigidMotion pridict;
//        pridict.translationMotion.tv  = front.translationMotion.ta*t+front.translationMotion.tv;
//        pridict.pose.t = front.pose.t + front.translationMotion.tv*t;
//        Eigen::Vector3d dt;
//        for (int i=0; i<3; ++i) {
//            dt[i] = 1/2 * (front.translationMotion.ta[i],2)*t;
//        }
//        pridict.pose.t += dt;
//        cout<<"last:"<<front.pose.t<<endl;
//        cout<<"measurement:"<<back.pose.t<<endl;
//        cout<<"predict:"<<pridict.pose.t<<endl;
//        fout<<front<<endl;
//    }
//    
//}